//
// Created by LiuDongPeng on 2023/10/27.
//

#include "bsp.h"
#include "stdio.h"

/* ST HAL Lib */
#include "main.h"
#include "tim.h"
#include "spi.h"
//#include "i2c.h"
#include "gpio.h"
#include "adc.h"
#include "opamp.h"

#include "lowpass_filter.h"



/**
 * @brief 10K flash to store system config
 */
#define SYSTEM_FLASH_END_ADDRESS ((__IO uint32_t)0x8020000)
#define SYSTEM_FLASH_CONFIG_START_ADDRESS ((__IO uint32_t)0x801D800)





/**
 * @brief 控制板上的led
 */
led_t bsp_blink_led, bsp_status_led;

// 闪烁led
#define LED_BLINK_GPIO_PORT  LED_BLINK_GPIO_Port
#define LED_BLINK_GPIO_PIN   LED_BLINK_Pin

// 状态led
#define LED_STATUS_GPIO_PORT  LED_STATUS_GPIO_Port
#define LED_STATUS_GPIO_PIN   LED_STATUS_Pin

static void led_blink_set_level(uint8_t level)
{
    HAL_GPIO_WritePin(LED_BLINK_GPIO_PORT, LED_BLINK_GPIO_PIN, (level));
}

static void led_status_set_level(uint8_t level)
{
    HAL_GPIO_WritePin(LED_STATUS_GPIO_PORT, LED_STATUS_GPIO_PIN, (level));
}


/**
 * @brief 控制板上的按键
 */
button_t bsp_btn1, bsp_btn2, bsp_btn3;

// btn1
#define BTN1_GPIO_PORT  BTN1_GPIO_Port
#define BTN1_GPIO_PIN   BTN1_Pin

// btn2
#define BTN2_GPIO_PORT  BTN2_GPIO_Port
#define BTN2_GPIO_PIN   BTN2_Pin

// btn3
#define BTN3_GPIO_PORT  BTN3_GPIO_Port
#define BTN3_GPIO_PIN   BTN3_Pin

static uint8_t get_btn1_level()
{
    return HAL_GPIO_ReadPin(BTN1_GPIO_PORT, BTN1_GPIO_PIN);
}

static uint8_t get_btn2_level()
{
    return HAL_GPIO_ReadPin(BTN2_GPIO_PORT, BTN2_GPIO_PIN);
}

static uint8_t get_btn3_level()
{
    return HAL_GPIO_ReadPin(BTN3_GPIO_PORT, BTN3_GPIO_PIN);
}

/**
 * @brief 按键扫描时钟滴答，5ms调用一次
 * @return
 */
void bsp_btn_ticks()
{
    button_ticks();
}



/**
* @brief AS5600磁编码器
*/
as5600_t bsp_as5600;

#define AS5600_I2C_HANDLE   hi2c1

static int i2c_mem_write(uint16_t dev_addr, uint16_t mem_addr, uint16_t mem_addr_size,
                         uint8_t *buf, uint16_t btw, uint32_t timeout)
{
//    return HAL_I2C_Mem_Write_DMA(&AS5600_I2C_HANDLE, AS5600_I2C_ADDRESS << 1, mem_addr, mem_addr_size, buf, btw);
//    return HAL_I2C_Mem_Write(&AS5600_I2C_HANDLE, AS5600_I2C_ADDRESS << 1, mem_addr, mem_addr_size, buf, btw, timeout);
}

static int i2c_mem_read(uint16_t dev_addr, uint16_t mem_addr, uint16_t mem_addr_size,
                        uint8_t *buf, uint16_t btr, uint32_t timeout)
{
//    return HAL_I2C_Mem_Read_DMA(&AS5600_I2C_HANDLE, AS5600_I2C_ADDRESS << 1, mem_addr, mem_addr_size, buf, btr);
//    return HAL_I2C_Mem_Read(&AS5600_I2C_HANDLE, AS5600_I2C_ADDRESS << 1, mem_addr, mem_addr_size, buf, btr, timeout);
}


/**
 * @brief AS5047P磁编码器
 */
as5047p_t bsp_as5047p;

#define AS5047P_SPI_HANDLE  hspi1
#define AS5047P_SPI_CS_PORT GPIOA
#define AS5047P_SPI_CS_PIN  GPIO_PIN_4

static int encoder_spi_send_recv_callback(uint8_t* pTxData, uint8_t* pRxData, uint16_t size, uint32_t timeOut)
{
    return HAL_SPI_TransmitReceive(&AS5047P_SPI_HANDLE, pTxData, pRxData, size, timeOut);
}
static void encoder_spi_cs_set_callback(uint8_t val)
{
    HAL_GPIO_WritePin(AS5047P_SPI_CS_PORT, AS5047P_SPI_CS_PIN, val);
}


/**
 * @brief MT6701磁编码器
 */
mt6701_t bsp_mt6701;


/**
 * @brief 编码器
 */
encoder_t encoder1;

static int encoderGetDataCallback(encoder_t* encoder, uint16_t* raw, float* angle, float* angleRad)
{
//    return as5047p_get_data(&bsp_as5047p, raw, angle, angleRad);
//    return as5600_get_angle(&bsp_as5600, angle, angleRad);
    return mt6701_get_data(&bsp_mt6701, raw, angle, angleRad);
}




/**
 * @brief 测速用的微秒级定时器
 */
#define MOTOR_VELOCITY_TIM_HANDLE  htim16

/**
 * @brief 使用定时器获取微秒级时钟滴答
 * @return
 */
uint32_t bsp_get_us_tick()
{
    return __HAL_TIM_GET_COUNTER(&MOTOR_VELOCITY_TIM_HANDLE);
}



/**
 * @brief 电机
 */
motor_t motor1;


/**
 * @brief 电机开发板
 */
foc_board_t bsp_board;


/**
 * @brief 开发板硬件外设初始化
 * @return
 */
int bsp_init()
{
    int ret = 0;

    /* AS5600初始化 */
    ret = as5600_create(&bsp_as5600, i2c_mem_write, i2c_mem_read);

    /* AS5047P初始化 */
    ret = as5047p_init(&bsp_as5047p, encoder_spi_send_recv_callback, encoder_spi_cs_set_callback);

    /* MT6701初始化 */
    ret = mt6701_init(&bsp_mt6701, encoder_spi_send_recv_callback, encoder_spi_cs_set_callback);

    /* 编码器初始化 */
    ret = encoder_init(&encoder1, encoderGetDataCallback);

    /* 电机初始化 */
    ret = motor_create(&motor1);
    motor_link_encoder(&motor1, &encoder1);

    /* 按键初始化 */
    ret = button_create(&bsp_btn1, 1, get_btn1_level);
    ret = button_create(&bsp_btn2, 1, get_btn2_level);
    ret = button_create(&bsp_btn3, 1, get_btn3_level);

    /* led初始化 */
    ret = led_create(&bsp_blink_led, LED_LEVEL_HIGH, led_blink_set_level);
    ret = led_create(&bsp_status_led, LED_LEVEL_HIGH, led_status_set_level);

    /* 使能运放 */
    HAL_OPAMP_Start(&hopamp1);
//    HAL_OPAMP_Start(&hopamp2);
    HAL_OPAMP_Start(&hopamp3);

    /* ADC自校准 */
    HAL_ADCEx_Calibration_Start(&hadc1, ADC_SINGLE_ENDED);
    HAL_ADCEx_Calibration_Start(&hadc2, ADC_SINGLE_ENDED);

    /* 开启ADC注入组转换中断 */
    HAL_ADCEx_InjectedStart_IT(&hadc1);
//    HAL_ADCEx_InjectedStart(&hadc2);

    /* 关闭PWM输出 */
    HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_1);
    HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_2);
    HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_3);
    HAL_TIMEx_PWMN_Start(&htim1, TIM_CHANNEL_1);
    HAL_TIMEx_PWMN_Start(&htim1, TIM_CHANNEL_2);
    HAL_TIMEx_PWMN_Start(&htim1, TIM_CHANNEL_3);

    HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_4);

    /* 启动电机测速用的微秒计数定时器 */
//    HAL_TIM_Base_Start(&MOTOR_VELOCITY_TIM_HANDLE);
    HAL_TIM_Base_Start_IT(&htim3);

    return ret;
}


/**
 * @brief Get board u v w phase current
 * @param board
 * @param ia
 * @param ib
 * @param ic
 */
void bsp_get_phase_current(foc_board_t *board, float32_t *ia, float32_t *ib, float32_t *ic)
{
    uint32_t adcA = 0, adcB = 0, adcC = 0;

    if (board == NULL || ia == NULL || ib == NULL || ic == NULL)
        return;

    adcA = HAL_ADCEx_InjectedGetValue(&hadc1, ADC_INJECTED_RANK_1);
    adcC = HAL_ADCEx_InjectedGetValue(&hadc1, ADC_INJECTED_RANK_2);
//    adcB = HAL_ADCEx_InjectedGetValue(&hadc2, ADC_INJECTED_RANK_1);

    *ia = ((float)adcA - (float)board->iaOffset) * 0.01933593f;
    *ic = ((float)adcC - (float)board->icOffset) * 0.01933593f;
//    *ib = ((float)adcB - (float)board->ibOffset) * 0.01933593f;

//	motor1.phaseCurrent.ia = low_pass_filter_work(&motor1.iaLpFilter, motor1.phaseCurrent.ia);
//	motor1.phaseCurrent.ic = low_pass_filter_work(&motor1.icLpFilter, motor1.phaseCurrent.ic);

    *ib = -(*ia) - (*ic);
}

/**
 * @brief Get board u v w phase current at present sector
 * @param board
 * @param sector
 * @param ia
 * @param ib
 * @param ic
 */
void bsp_get_phase_current_with_sector(foc_board_t *board, int sector, float *ia, float *ib, float *ic)
{
    uint32_t adcA, adcB, adcC;

    if (board == NULL || ia == NULL || ib == NULL || ic == NULL)
        return;

    if (sector > 6)
        sector = 6;
    if (sector < 1)
        sector = 1;

    switch (sector)
    {
        case 4:
        case 5:
            /* Ia Ib < 0 */
            adcA = HAL_ADCEx_InjectedGetValue(&hadc1, ADC_INJECTED_RANK_1);
            adcB = HAL_ADCEx_InjectedGetValue(&hadc2, ADC_INJECTED_RANK_1);
            *ia = (float) (adcA - board->iaOffset) * 0.01933593f;
            *ib = (float) (adcB - board->ibOffset) * 0.01933593f;
            *ic = -*ia - *ib;
            break;

        case 1:
        case 6:
            /* Ib Ic < 0 */
            adcC = HAL_ADCEx_InjectedGetValue(&hadc1, ADC_INJECTED_RANK_2);
            adcB = HAL_ADCEx_InjectedGetValue(&hadc2, ADC_INJECTED_RANK_1);
            *ic = (float) (adcC - board->icOffset) * 0.01933593f;
            *ib = (float) (adcB - board->ibOffset) * 0.01933593f;
            *ia = -*ib - *ic;
            break;


        case 2:
        case 3:
        default:
            /* Ia Ic < 0 */
            adcA = HAL_ADCEx_InjectedGetValue(&hadc1, ADC_INJECTED_RANK_1);
            adcC = HAL_ADCEx_InjectedGetValue(&hadc1, ADC_INJECTED_RANK_2);
            *ia = (float) (adcA - board->iaOffset) * 0.01933593f;
            *ic = (float) (adcC - board->icOffset) * 0.01933593f;
            *ib = -*ia - *ic;
            break;
    }
}


/**
 * @brief
 * @param motor
 */
void bsp_store_encoder_angle_offset(motor_t* motor)
{
    HAL_FLASH_Unlock();
    HAL_FLASH_Program(FLASH_TYPEPROGRAM_DOUBLEWORD, SYSTEM_FLASH_CONFIG_START_ADDRESS, (uint64_t)motor->angleRadOffset);
    FLASH_WaitForLastOperation(500);
    HAL_FLASH_Lock();
}

/**
 * @brief
 * @param[out]  motor
 * @return
 */
float bsp_restore_encoder_angle_offset(motor_t* motor)
{
    if (motor != NULL)
    {
        motor->angleRadOffset = (float) *((__IO uint64_t *) SYSTEM_FLASH_CONFIG_START_ADDRESS);
    }
}
